#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S3,     Ultra,          sensorSONAR)
#pragma config(Sensor, S4,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     Lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorG,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     B_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     F_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     B_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     F_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Launcher,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     Raise,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C3_1,    Hook,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C3_2,    Guide,                tServoStandard)
#pragma config(Servo,  srvo_S2_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_6,    Scooper,              tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "AutonomousBase.c"

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
 motor[B_Right] = 0;					//Reset all motor values.
	motor[B_Left] = 0;
	motor[F_Right] = 0;
	motor[F_Left] = 0;
	//motor[Launch] = 0;
	//nMotorEncoder[Launch] = 0;
	resetEncoders();
	servoTarget[Hook] = 50;			//Sets hook up.
	servoTarget[Scooper] = 175;

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

//drive_full(char dir, int dist, int speed, int lExt, int scoopT, float dScoop, int raised, int maintain, int maint_range)
task main()
{
	int sector;
  initializeRobot();

  //waitForStart(); // Wait for the beginning of autonomous phase.

 drive_full(0, 50, 50, 0, 175, 0, 1, 0, 0);
 drive_full(0, 38, 80, 0, 175, 0, 1, 0, 0);
 hooke(1);
 wait10Msec(75);
 rotate(1, 25);
 drive_full(1, 95, 80, 1, 0, 5, 1, 0, 0);
 rotate(0, 190);
 drive(0, 3);
 hooke(0);
 //wait10Msec(15);
 drive(0, 4);
 rotate(0, 110);
 sector = getIRSector();
 writeDebugStreamLine("%d", sector);
 if(sector == 21)
 {
   seekIR(73, 0);
 	 //launch(4);
 }

 if(sector == 10)
 {
   rotate(0, 60);
   drive(0, 40);
   rotate(1, 90);
   seekIR(73, 0);
   drive(1, 4);
 	 //launch(4);
 	}





}
